#pragma once

#include "g2o/core/base_multi_edge.h"
#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_vec.hpp"
#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_q.hpp"

namespace g2o
{
    class EdgeMarginalization : public BaseMultiEdge<-1, std::pair<Eigen::MatrixXd, Eigen::VectorXd>>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        EdgeMarginalization();
        virtual void setDimension(int dimension_);
        virtual void setSize(int vertices);
        virtual bool read(std::istream &is) { return false; }
        virtual bool write(std::ostream &os) const { return false; }
        void computeError();
        virtual void linearizeOplus();
        virtual void setMeasurement(const std::pair<Eigen::MatrixXd, Eigen::VectorXd> &m);
        std::vector<g2o::MatrixX::MapType, Eigen::aligned_allocator<g2o::MatrixX::MapType>> GetJacobian() const
        {
            return _jacobianOplus;
        }
        void push_back(int index, const Eigen::Vector3d &v);
        void push_back(int index, const Eigen::Quaterniond &q);
        std::unordered_map<int, Eigen::Vector3d> vecs_;
        std::unordered_map<int, Eigen::Quaterniond> qs_;
    };
} // namespace g2o